﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using System.Windows.Media.Media3D;
using Microsoft.Kinect;
using Microsoft.Samples.Kinect.WpfViewers;
using Microsoft.Kinect.Toolkit;
using BodyGestIT;
using GestIT;

namespace CarViewer
{
    /// <summary>
    /// Interaction logic for CarViewer.xaml
    /// </summary>
    public partial class CarWindow : Window
    {
        double maxDiff = 0.1;
        private readonly KinectSensorChooser sensorChooser;
        public StateChangedHandler h1;
        public CarWindow()
        {
            sensorChooser = new KinectSensorChooser();
            this.KinectSensorManager = new KinectSensorManager();
            this.KinectSensorManager.KinectSensorChanged += new EventHandler<KinectSensorManagerEventArgs<KinectSensor>>(KinectSensorManager_KinectSensorChanged);
            this.DataContext = this.KinectSensorManager;
            InitializeComponent();

            this.kinectSensorChooser1.KinectSensorChooser = sensorChooser;
 
            // Bind the KinectSensor from the sensorChooser to the KinectSensor on the KinectSensorManager
            var kinectSensorBinding = new Binding("Kinect") { Source = this.sensorChooser };
            BindingOperations.SetBinding(this.KinectSensorManager, KinectSensorManager.KinectSensorProperty, kinectSensorBinding);

            
            sensorChooser.Start();
            
            

            if (sensorChooser.Kinect != null && sensorChooser.Kinect.Status == KinectStatus.Connected)
            {
                Console.WriteLine("Recognition started");
                InitGesture();
                emitter.StartRecognition(new BodyToken(2));
            }
        }

        void KinectSensorManager_KinectSensorChanged(object sender, KinectSensorManagerEventArgs<KinectSensor> e)
        {
            InitializeKinectServices(KinectSensorManager, e.NewValue);
        }

        public static readonly DependencyProperty KinectSensorManagerProperty =
            DependencyProperty.Register(
                "KinectSensorManager",
                typeof(KinectSensorManager),
                typeof(MainWindow),
                new PropertyMetadata(null));


        BodyEmitter emitter;

        public KinectSensorManager KinectSensorManager
        {
            get { return (KinectSensorManager)GetValue(KinectSensorManagerProperty); }
            set { SetValue(KinectSensorManagerProperty, value); }
        }

        private void InitGesture()
        {
            emitter = new BodyEmitter(kinectSensorChooser1.KinectSensorChooser.Kinect, true);
            TmpExp gesture = moveCar.CreateTemporalExpression();
            emitter.SetGestureDescription(gesture);
            Console.WriteLine("Recognition started");
        }

        void openHand3_Completed(TmpExp sender, ExpEventArgs args, Token token)
        {
            //label1.Content = "Not Tracking";
            MoveArrow.Visibility = System.Windows.Visibility.Hidden;
            //label1.Background = (SolidColorBrush)this.Resources["redBackground"];
        }

        void gesture_Completed(TmpExp sender, ExpEventArgs args, Token token)
        {
            Console.WriteLine("Gesture complete");
        }

        void gesture_Error(TmpExp sender, ExpEventArgs args, Token token)
        {
            Console.WriteLine("Gesture error");
        }

        void openHand4_Completed(TmpExp sender, ExpEventArgs args, Token token)
        {
            //label1.Content = "Not Tracking";
            Rotate.Visibility = System.Windows.Visibility.Hidden;
            MoveArrow.Visibility = System.Windows.Visibility.Hidden;
            //label1.Background = (SolidColorBrush)this.Resources["redBackground"];
        }

        void rotateHandOpen_Completed(TmpExp sender, ExpEventArgs args, Token token)
        {
            Rotate.Visibility = System.Windows.Visibility.Visible;
            MoveArrow.Visibility = System.Windows.Visibility.Hidden;
        }
       

        void openHand_Completed(TmpExp sender, ExpEventArgs args, Token token)
        {
            MoveArrow.Visibility = System.Windows.Visibility.Visible;

            //label1.Content = "Tracking";
            //label1.Background = (SolidColorBrush) this.Resources["greenBackground"];
            //carControl1.Reset();
        }

        void grabExp_Completed(TmpExp sender, ExpEventArgs args, Token token)
        {
            label1.Content = "Open"; 
        }

        double scaleFactor = 1000.0;

        private bool rightHandOpen(BodyEventArgs args, BodyToken token)
        {
            return args.Skeleton.RightHandOpen;
        }

        private bool rightHandClosed2(BodyEventArgs args, BodyToken token)
        {
          
            return !args.Skeleton.RightHandOpen;
         
        }

        private bool rightHandClosed(BodyEventArgs args, BodyToken t)
        {
            
                return ! args.Skeleton.RightHandOpen;
     
        }

        private bool front(BodyEventArgs args, BodyToken t)
        {
            return Math.Abs(
                args.Skeleton.Joints[JointType.ShoulderLeft].Position.Z -
                 args.Skeleton.Joints[JointType.ShoulderRight].Position.Z) < 0.05; 
        }

        private bool onlyRightHandClosed(BodyEventArgs args, BodyToken token)
        {
            //return !args.Skeleton.RightHandOpen && args.Skeleton.LeftHandOpen;
            return !args.Skeleton.RightHandOpen;
        }

        private bool leftHandClosed(BodyEventArgs args, BodyToken token)
        {

            return !args.Skeleton.LeftHandOpen;

        }

        private bool rightHandOpened(BodyEventArgs args, BodyToken token)
        {
        
                return args.Skeleton.RightHandOpen;
   
        }
        private bool leftHandOpened(BodyEventArgs args, BodyToken token)
        {

            return args.Skeleton.LeftHandOpen;

        }

       

        void leftHandClosed_Completed(TmpExp sender, ExpEventArgs args, Token t)
        {
            Console.WriteLine("Left hand closed completed");
        }

        void rightHandClosed_Completed(TmpExp sender, ExpEventArgs args, Token t)
        {
            Console.WriteLine("Right hand closed completed");
        }


        void rightHandMoveExp_Completed(TmpExp sender, ExpEventArgs args, Token t)
        {
            BodyToken token = (BodyToken)t;
            BodyEventArgs bargs = (BodyEventArgs)args;
            Skeleton curr = token.GetSkeleton(bargs.Skeleton.TrackingId, 0);
            Skeleton prev = token.GetSkeleton(bargs.Skeleton.TrackingId, 1);


            if (prev == null || curr == null)
            {
                return;
            }


            double diffX =
                (curr.Joints[JointType.HandRight].Position.X -
                prev.Joints[JointType.HandRight].Position.X) * scaleFactor;

            double diffY = (curr.Joints[JointType.HandRight].Position.Y -
                prev.Joints[JointType.HandRight].Position.Y) * scaleFactor;

            carControl1.FrontOR30.Position = new Point3D(
                carControl1.FrontOR30.Position.X - diffX,
                carControl1.FrontOR30.Position.Y - diffY,
                carControl1.FrontOR30.Position.Z);

            Console.WriteLine("Pan");
            //carControl1.Move(createFrame(curr));
        }

        void rightHandMoveExp_RotationCompleted(TmpExp sender, ExpEventArgs args, Token t)
        {
            BodyToken token = (BodyToken)t;
            BodyEventArgs bargs = (BodyEventArgs)args;
            Skeleton curr = token.GetSkeleton(bargs.Skeleton.TrackingId, 0);
            carControl1.Move(createFrame(curr));
            Console.WriteLine("Rotation");
        }


        Point createFrame(Skeleton curr)
        {
            float l = 2 * Math.Abs(curr.Joints[JointType.Head].Position.Y - curr.Joints[JointType.HipCenter].Position.Y);

            Point zero = new Point(
                curr.Joints[JointType.HipCenter].Position.X - l,
                curr.Joints[JointType.HipCenter].Position.Y - l);

            Point toRet = new Point(
                (curr.Joints[JointType.HandRight].Position.X - zero.X) / (2 *l),
                (2 * l - (curr.Joints[JointType.HandRight].Position.Y - zero.Y)) / (2 *l));
            return toRet;
        }
        void notScreenFrontExp_Completed(TmpExp sender, ExpEventArgs args, Token token)
        {
            Console.WriteLine("Completed");
            
            //label1.Content = "Not Tracking";
        }

        void screenFrontExp_Completed(TmpExp sender, ExpEventArgs args, Token token)
        {
            label1.Content = "Tracking";
            //MoveArrow.Visibility = System.Windows.Visibility.Visible;
            label1.Background = (SolidColorBrush)this.Resources["greenBackground"];
        }

        double threshold = 0.001;
        private bool rightHandMove(BodyEventArgs args, BodyToken token)
        {
           
            return true;
            
        }

        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            kinectSensorChooser1.KinectSensorChooser.KinectChanged += new EventHandler<Microsoft.Kinect.Toolkit.KinectChangedEventArgs>(KinectSensorChooser_KinectChanged);
        }

        void KinectSensorChooser_KinectChanged(object sender, Microsoft.Kinect.Toolkit.KinectChangedEventArgs args)
        {
            if (null != args.OldSensor)
            {
                this.UninitializeKinectServices(args.OldSensor);
            }

            if (null != args.NewSensor)
            {
                this.InitializeKinectServices(this.KinectSensorManager, args.NewSensor);
                InitGesture();
                emitter.StartRecognition(new BodyToken(2));
            }

           
        }

        private void InitializeKinectServices(KinectSensorManager kinectSensorManager, KinectSensor sensor)
        {
            // Application should enable all streams first.
            kinectSensorManager.ColorFormat = ColorImageFormat.RgbResolution640x480Fps30;
            kinectSensorManager.ColorStreamEnabled = true;

            //sensor.SkeletonFrameReady += this.SkeletonsReady;
            kinectSensorManager.TransformSmoothParameters = new TransformSmoothParameters
            {
                Smoothing = 0.5f,
                Correction = 0.5f,
                Prediction = 0.5f,
                JitterRadius = 0.05f,
                MaxDeviationRadius = 0.04f
            };
            kinectSensorManager.SkeletonStreamEnabled = true;
            kinectSensorManager.KinectSensorEnabled = true;
        }

        // Kinect enabled apps should uninitialize all Kinect services that were initialized in InitializeKinectServices() here.
        private void UninitializeKinectServices(KinectSensor sensor)
        {
            if (sensor != null)
            {
                if (sensor.IsRunning)
                {
                    //stop sensor 
                    sensor.Stop();

                    //stop audio if not null
                    if (sensor.AudioSource != null)
                    {
                        sensor.AudioSource.Stop();
                    }


                }
            }
        }
        

        private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
        {
            // cleaning all references

            sensorChooser.Stop();
            this.KinectSensorManager.KinectSensor = null;
            //kinectSensorChooser1.KinectSensorChooser.Kinect = null;
        }

        private void Window_Closed(object sender, EventArgs e)
        {
            this.KinectSensorManager.KinectSensor = null;
        }

    }
}
